#include "bmi088.h"

#include <rtdevice.h>

using namespace NXMFC;

#define BMI08X_ACCEL_CHIP_ID_REG    0x00
#define BMI08X_ACCEL_CHIP_ID 0x1E

#define BMI08X_GYRO_CHIP_ID_REG     0x00
#define BMI08X_GYRO_CHIP_ID         0x0F

#define GYRO_SOFTRESET   0x14

#define RATE_X_LSB     0x02

#define ACCEL_SOR4  0x08
#define ACCEL_SOR2  0x09
#define ACCEL_SOR0  0x0A

#define ACCEL_ODR_12_5HZ  	0x05
#define ACCEL_ODR_25HZ  		0x06
#define ACCEL_ODR_50HZ 		  0x07
#define ACCEL_ODR_100HZ  		0x08
#define ACCEL_ODR_200HZ  		0x09
#define ACCEL_ODR_400HZ  		0x0A
#define ACCEL_ODR_800HZ  		0x0B
#define ACCEL_ODR_1600HZ  	0x0C

bmi088::bmi088()
{
    _acc = 0;
    _gry = 0;
}

bmi088::~bmi088()
{

}

bool bmi088::open(const char *acc, const char *gry)
{
    _acc = (struct rt_spi_device *)rt_device_find(acc);
    _gry = (struct rt_spi_device *)rt_device_find(gry);

    if (!_acc || !_gry)
        return false;

    if (!init())
        return false;

    return true;
}

bool bmi088::readAccel(short &x, short &y, short &z)
{
    uint8_t buf[7];

    accelReadRegs(0x12, buf, 7);

    x = ((int16_t)((buf[2] << 8) | buf[1]));
    y = ((int16_t)((buf[4] << 8) | buf[3]));
    z = ((int16_t)((buf[6] << 8) | buf[5]));

    return true;
}

bool bmi088::readGyro(short &x, short &y, short &z)
{
    uint8_t buf[6];

    gyroReadRegs(RATE_X_LSB, buf, 6);

    x = ((int16_t)((buf[1] << 8) | buf[0]));
    y = ((int16_t)((buf[3] << 8) | buf[2]));
    z = ((int16_t)((buf[5] << 8) | buf[4]));

    return true;
}

bool bmi088::init()
{
    uint8_t id;
    uint8_t ACC_CONF = 0;

	ACC_CONF |= 0x80;//设置最高位为1
	ACC_CONF |= ACCEL_SOR4<<4;
	ACC_CONF |= ACCEL_ODR_200HZ;

    id = accelReadID();
    if (id != BMI08X_ACCEL_CHIP_ID)
        return false;

    id = gyroReadID();
    if (id != BMI08X_GYRO_CHIP_ID)
        return false;

    writeReg(BMI08X_ACCEL_CHIP_ID, 0x7D, 0x04); //开启加速度计
    rt_thread_mdelay(5);
    writeReg(BMI08X_ACCEL_CHIP_ID, 0x7C, 0x00); //功率模式设置
    rt_thread_mdelay(5);
    writeReg(BMI08X_ACCEL_CHIP_ID, 0x41, 0x03); //加速度量程,量程越大越抗震
    rt_thread_mdelay(5);
    writeReg(BMI08X_ACCEL_CHIP_ID, 0x40, ACC_CONF); //滤波和采样率设置
    rt_thread_mdelay(5);

    writeReg(BMI08X_GYRO_CHIP_ID, 0x0f, 0x00);
    rt_thread_mdelay(5);
    writeReg(BMI08X_GYRO_CHIP_ID, 0x10, 0x01);
    rt_thread_mdelay(5);
    writeReg(BMI08X_GYRO_CHIP_ID, 0x11, 0x00);
    rt_thread_mdelay(5);

    return true;
}

uint8_t bmi088::accelReadID()
{
    uint8_t val[2];

    accelReadRegs(BMI08X_ACCEL_CHIP_ID_REG, val, 2);

    return val[1];
}

void bmi088::accelReadRegs(uint8_t addr, uint8_t *data, uint16_t len)
{
    uint8_t buf = 0;
    struct rt_spi_message m = {0};

    addr |= 0x80;

    m.send_buf = &addr;
    m.length = 1;

    rt_spi_take_bus(_acc);
    rt_spi_take(_acc);

    rt_spi_transfer_message(_acc, &m);

    m.send_buf = &buf;
    for (int i = 0; i < len; i ++)
    {
        m.recv_buf = &data[i];

        rt_spi_transfer_message(_acc, &m);
    }

    rt_spi_release(_acc);
    rt_spi_release_bus(_acc);
}

void bmi088::gyroReadRegs(uint8_t addr, uint8_t *data, uint16_t len)
{
    uint8_t buf = 0;
    struct rt_spi_message m = {0};

    addr |= 0x80;

    m.send_buf = &addr;
    m.length = 1;

    rt_spi_take_bus(_gry);
    rt_spi_take(_gry);

    rt_spi_transfer_message(_gry, &m);

    m.send_buf = &buf;
    for (int i = 0; i < len; i ++)
    {
        m.recv_buf = &data[i];

        rt_spi_transfer_message(_gry, &m);
    }

    rt_spi_release(_gry);
    rt_spi_release_bus(_gry);
}

uint8_t bmi088::gyroReadID()
{
    uint8_t val[1];

    gyroReadRegs(BMI08X_GYRO_CHIP_ID_REG, val, 1);

    return val[0];
}

void bmi088::writeReg(int id, uint8_t addr, uint8_t val)
{
    struct rt_spi_device *d;
    struct rt_spi_message m = {0};

    if (id == BMI08X_ACCEL_CHIP_ID)
        d = _acc;
    else
        d = _gry;

    m.send_buf = &addr;
    m.length = 1;

    rt_spi_take_bus(d);
    rt_spi_take(d);

    rt_spi_transfer_message(d, &m);

    m.send_buf = &val;
    rt_spi_transfer_message(d, &m);

    rt_spi_release(d);
    rt_spi_release_bus(d);
}
